'''
Created on Feb 11, 2012

@author: zahy
'''


'''
TODO create maps (handler) from a multiagent instance 
TODO Implement the cr-table and the fix-a*  
'''
from astar import AStar, SQ_MapHandler, SQ_Location, EdgeNode, Lid


class Conflict: 
    def __init__(self,lid,time,bids):
        self.lid = lid
        self.time = time
        self.bids = bids
    def __str__(self):
        return str(self.lid)+"@"+str(self.time)
        
def printPaths(agentsCurrentPaths):
    for agent in agentsCurrentPaths:
        print "Agent"+str(agent),
        for step in agentsCurrentPaths[agent].getNodes():
            if not isinstance(step,EdgeNode):
                print str(step.location.x)+ "," + str(step.location.y)+" ",
        print agentsCurrentPaths[agent].getTotalMoveCost()
                
            
def printBids(bids):
    print "***BIDS****"    
    for bid in bids:
        source = bid.source
        sy = source/width
        sx = source % width 
        target = bid.target
        if target == -1:
            ty = tx = '.'
        else:             
            ty = target/width
            tx = target % width
        print str(sx)+","+str(sy)+ "->" + str(tx)+","+str(ty) + str(bids[bid])

#    sorted_ = sorted(bids.iteritems(), key=operator.itemgetter(1))
#    for b in sorted_:
#        print str(b[0])+"   :"+str(b[1])

#does this bids table has conflicts 
def hasConflicts(currentAgentPaths):
    if len(currentAgentPaths) == 0:
        return True
    nodeSet = set()    
    for path in currentAgentPaths.values():
        for n in path.getNodes(): 
            if n in nodeSet:
                return True
            else:
                nodeSet.add(n)
    return False


#choose a conflict from the table                
def chooseConflict(currentAgentPaths):
    if len(currentAgentPaths) == 0:
        return None
    nodeSet = set()    
    for path in currentAgentPaths.values():
        for n in path.getNodes(): 
            if n in nodeSet:
                return Conflict(n.lid,n.time,None)
            else:
                nodeSet.add(n)

# remove the bids
def removeBids(agentId,agentCurrentPath, bids):
    for n in agentCurrentPath.getNodes():
        bidList = bids[n.lid][n.time] #dict {Agent: bid}
        if agentId not in bidList:
            raise Exception("Agent id not in bids")         
        del bidList[agentId]

            
#updates the bid table with agent path 
def updateBids(agentId,agentPath, bids):    
    t=1
    for n in agentPath.getNodes():
        if not n.lid in bids.keys():
            bids[n.lid]=[{} for __ in range(len(agentPath.getNodes())+1)]
        if t>=len(bids[n.lid]):
            bids[n.lid].extend([{} for __ in range(t-len(bids[n.lid])+1)])
        if agentId not in bids[n.lid][t].keys():
            bids[n.lid][t][agentId]=0 #bid of 0
        t=t+1
#plan 
def plan(a,agents,agentCurrentPaths,bids,conflict): 
    for agent in agents:
        if agent in agentCurrentPaths:
            currentPath = agentCurrentPaths[agent]
        else:  
            currentPath = None
        p = a.findPath(agents[agent][0],agents[agent][1],bids,agent,currentPath)
        agentCurrentPaths[agent] = p    
    #printBids(bids)
    for agent in agents:
        updateBids(agent,agentCurrentPaths[agent],bids)

def mapf(agents,mapdata,width,height):
    a = AStar(SQ_MapHandler(mapdata, width, height))
    bids={} #its a dict of list of dicts
    agentCurrentPaths={} # path of agents
    while hasConflicts(agentCurrentPaths):
        conflict = chooseConflict(agentCurrentPaths) # (lid,time)->(agent,bid),(agent,bid),(agent,bid)
        print("Conflict: "+str(conflict))
        if conflict != None:                 
            for agent in agents: #only on those of the conflict
                pAlt = a.findPath(agents[agent][0],agents[agent][1],bids,agent,agentCurrentPaths[agent],conflict)
                bid  = pAlt.getTotalMoveCost() - agentCurrentPaths[agent].getTotalMoveCost()        
                bids[conflict.lid][conflict.time][agent] = max(bid,1)
        #printBids(bids)        
        plan(a, agents, agentCurrentPaths, bids,conflict)
        printPaths(agentCurrentPaths)
        #printBids(bids)
    return agentCurrentPaths
        
        
#mapdata = [1,1,1,
#           -1,-1,1,
#           1,1,1]
#width=3
#height=3    
#agents={1:(SQ_Location(0, 0), SQ_Location(2, 2)),
#        2:(SQ_Location(0, 0), SQ_Location(2, 2))}

# Simple example 1 
#mapdata = [1,-1,1,
#           1, 1,1,
#           1,-1,1]
#width=3
#height=3    
#agents={1:(SQ_Location(0, 0), SQ_Location(2, 2)),
#        2:(SQ_Location(2, 0), SQ_Location(0, 2))}

mapdata = [1,1,
           1,1]
width=2
height=2    
agents={1:(SQ_Location(0, 0), SQ_Location(1, 0)),
        2:(SQ_Location(1, 0), SQ_Location(0,0))}

#Test the scenario
a = AStar(SQ_MapHandler(mapdata, width, height))
lid = Lid(0,1)
bids={}
bids[lid]=[{} for __ in range(5)] 
bids[lid][1][2] = 1.001
p =a.findPath(agents[2][0], agents[2][1],bids, 2, None, None)
print p
print p.getTotalMoveCost()
pass
exit()

agentsCurrentPaths = mapf(agents,mapdata,width,height)
for p in agentsCurrentPaths: 
    print "agent"+str(p)+": "+str(agentsCurrentPaths[p].getTotalTime())

#a = AStar(SQ_MapHandler(mapdata, width, height))
#p = a.findPath(agents[2][0], agents[2][1], {}, 2)
#printPaths({1:p})
    

#a = AStar(SQ_MapHandler(mapdata, width, height))
#p = a.findPath(SQ_Location(0, 0), SQ_Location(2, 2), {})
#print "**********No Conflict**********"
#for n in p.getNodes():
#    print n
#
#
#p = a.findPath(SQ_Location(0, 0), SQ_Location(2, 2), {}, Conflict((2,-1), 4, []))
#print "**********Conflict**********"
#for n in p.getNodes():
#    print n